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Abstract 

Robots in applications may be subject to holonomic or nonholonomic constraints. 
Examples of holonomic constraints include a manipulator constrained through the con- 
tact with the environment , e.g., inserting a part , turning a crank , etc. f and multi- 
ple manipulators constrained through a common payload . Examples of nonholonomic 
constraints include no-slip constraints on mobile robot wheels , local normal rotation 
constraints for soft finger and rolling contacts in grasping t and conservation of angular 
momentum of in-orbit space robots. The above examples all involve equality constraints; 
in applications , there are usually additional inequality constraints such as robot joint 
limits , self collision and environment collision avoidance constraints , steering angle 
constraints in mobile robots, etc. 

This paper addresses the problem of finding a kinematically feasible path that sat- 
isfies a given set of holonomic and nonholonomic constraints, of both equality and 
inequality types. The path planning problem is first posed as a finite time nonlin- 
ear control problem. This problem is subsequently transformed to a static root finding 
problem in an augmented space which can then be iteratively solved. The algorithm has 
shown promising results in planning feasible paths for redundant arms satisfying Carte- 
sian path following and goal endpoint specifications, and mobile vehicles with multiple 
trailers. In contrast to local approaches , this algorithm is less prone to problems such 
as singularities and local minima. 

1 Introduction 

Controlling robot motion, including both in a.iii pnla.tnrs and mobile vehicles, usually 
involves the following steps: 

1. Kinematic path planning: find a path that satisfies all the geometric specifications 
and constraints of a given task. 

2. Trajectory generation: index the path with time to generate a dynamic trajectory. 

3. Dynamic trajectory following: design a servo controller (possibly incorporating 
dynamic information of the overall system) to follow the trajectory. 


b 


This paper focuses on the kinematic path planning problem. In contrast to most of 
the existing algorithms which are local and reactive in nature, our approach is a global 
one which warps an entire path to satisfy all the constraints. A common classification of 

involves 1 hol ° nom ^ versus nonholonomic. If a constraint can be expressed in 
terms of the generahzed coordinate, it is holonomic; if a constraint involves the gener- 
alized velocity and it is not mtegrable, then the constraint is nonholonomic. Examples 
of holonomic constraints include a manipulator constrained through its contact with 

'T TtiRg a Part ’ tUrning a Crank ’ etc ‘’ and manipula- 

tosrs constrained tirough a common payload. Examples of nonholonomic constraints 

mdMe m-Ap on mobile vehicle wheels, local normal rotation constraints 

for soft finger contacts m grasping, and conservation of angular momentum of space 
mW There may be other design constraints imposed by the task, for example 
equality constraints such as the desired terminal configuration, specified end effector 
path, etc., and inequality constraints such as manipulator joint limits, self collision and 
environment collision avoidance constraints. 

There is abundant literature on path planning for redundant robots, which are 
examples of systems with holonomic constraints, and mobile robots which are examples 
of systems with nonholonomic constraints, but seldom on both. The principal reason 
is that in the local approach, the two problems are fundamentally different in that 
redundant robots can in general move in all directions locally in the configuration 
space, but nonholonomic systems can only move in certain directions. Consequently 
the issues related to redundant robots are singularity, redundancy resolution, joint 
cyclicity for cyclic end effector paths, etc., and, for nonholonomic systems, the main 
issue is in finding a path whose tangent lies within the admissible directions. There 
are also commonalities in the two classes of problems: 


1. Kinematic models are linear in control (i.e., admissible velocities). 

2. Collision avoidance and joint limits are represented as a set of inequality con 
straints. 


In our approach, the entire path is iterated toward a solution. Therefore, in this 
framework, whether the constraint is holonomic or nonholonomic does not make a 
fundamental difference; a more important distinction is between equality and inequality 
constraints. Indeed, path planning for redundant robots and mobile robots are treated 
in exactly the same way in our proposed approach. 

The manipulator path planning problem is traditionally based on geometric ap- 
proaches. (Comprehensive surveys can be found in [1, 2, 3].) The majority of these 
use the configuration representation of the manipulator and the obstacles and joint 
limits (as originally proposed in [4]). In this formulation, the path planning problem 
is reduced to finding a feasible path for a single point. Various tools from geometry, 
topology and algebra have been utilized to develop methods such as roadmap, cell 
decomposition, and potential field methods. The drawback of the configuration space 
approach is that the configuration space can become high dimensional for manipu- 
lators with several joints and mapping out the free region for a redundant arm in a 
cluttered environment is a time consuming process and produces large graphs to be 
searched. Recently, some work has emerged which focuses on minimizing this growth 
in computation by incorporating heuristics [5, 6]. 
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The specific issue of redundancy resolution in redundant manipulators has tradi- 
tionally been addressed by using the Jacobian pseudo-inverse [7, 8, 9, 10], which is 
a local, or point-wise on the path, approach. This approach is simple to use, has 
low computational overhead, and can be combined with the local potential field for 
collision avoidance. The drawbacks include local minima, breakdown near or at sin- 
gularities, and non-cyclic or even unstable joint motion for cyclic end effector paths 
[11, 12, 13, 14]. Some global techniques based on optimization have also been proposed 
[15, 16, 17, 18, 19], but the resulting two-point boundary value problem (TPBVP) is 
difficult to solve for complex problems. 

In the rapidly accumulating literature on nonholonomic motion planning, there 
appears to be a similar dichotomy of approaches: graph search based methods as 
in [20, 21, 22, 23, 24, 25, 26] and analytic methods [27, 28, 29, 30, 31, 32, 33, 34, 
35]. The former class tends to be computationally inefficient but can handle general 
constraints, the latter class gives elegant insights into the structure of the solution but 
may generate impractical paths. Recently, some important insights have been gained 
by linearizing the kinematics equation about a non-stationary trajectory instead of 
a fixed equilibrium. It is shown in [36] that the linearized time varying system is 
frequently controllable, and a locally stable feedback controller can be designed. In 
[37], a globally stable feedback controller is also found. Based on this approach, a 
general procedure for designing globally stable time varying feedback controller has 
been obtained [38], 

A common starting point for the analytic approaches to the path planning problem 
is to pose it as a general nonlinear control problem. Using this framework, we present 
a new method of solving the path planning problem in this paper. The formulation is 
based on converting the nonlinear control problem into a nonlinear algebraic equation. 
The problem then becomes a nonlinear root finding problem in which the dimension 
of the search space is very high (infinite if (u(t) : t £ [0, 1]} is taken from an infinite 
dimensional functional space) compared to the number of equality constraints (for 
the end point constraint). The nonlinear root finding problem is further converted to 
an initial value problem (IVP) which is much easier to solve than the usual TPBVP 
typically arising in the optimization approach. Under some additional assumptions, 
the IVP can be shown to be wellposed [39] and a variable step size ODE solver is used 
to propagate the solution. Examples ranging from a front-wheel driven car to triple 
trailers to nine degree-of-freedom (DOF) manipulators have been successfully tackled 
[40, 41, 42]. A similar approach has also been proposed independently in [43, 44] 
for kinematic path planning with only the end point constraint. For this case, the 
wellposedness property of the IVP is shown to be generic. 

Since the dimension of the search space is very high, there are many possible solu- 
tions to the root finding problem which results from the path planning problem. For 
any practical applications, additional constraints must be placed. We have adopted an 
approach similar to the global exterior penalty function method [ 45 , 46 ] which converts 
inequality constraints into a zero finding problem. This differs from the familiar arti- 
ficial potential field method which is an interior penalty function (or barrier function) 
method in that the initial guess may be infeasible. 

The inequality constrained case cam then be combined with the equality constraints 
into an augmented zero finding problem. Non-configuration space constraints, for 
example, constraints involving corners of a vehicle, body of the robot, etc., cam also be 
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incorporated in this formulation. 

The global path p lanning approach that we have proposed has been applied to 
examples involving redundant manipulators and nonholonomic vehicles. We note the 
following attractive features of this algorithm: 

1. Both equality and inequality constraints can be included in this formulation. 
The constraints can be nonlinear in the configuration variable, so task space 
constraints (which involve nonlinear kinematic function of the configuration vari- 
ables) are also allowed. 

2. The initial guess does not have to be feasible. The planner iteratively warps the 
path until all constraints are satisfied. 

3. This approach emphasizes feasibility over optimality in contrast to other global 
approaches. Once a feasible solution is found, optimality can be incorporated as 
a secondary constraint. 

4. The IVP formulation is computationally easier to solve than the TPBVP. 

5. There are additional points related specifically to redundant manipulators: 

• Goal task variability: This approach can be used for finding a feasible joint 
sequence from a fixed initial configuration to a specified Cartesian or joint 
space goal - the path planning problem - as well as for global redundancy 
resolution along a specified Cartesian path. 

• Singularity robustness: The global nature of the planner avoids the Jaco- 
bian singularity problem inherent in local methods. While the controllabil- 
ity about a configuration is lost at the singularity, the algorithm can proceed 
as long as controllability about the planned path, which is a much looser 
condition to satisfy, is retained. 

• Incorporation of additional equality constraints: As an application of this 
capability, the planner can generate cyclic joint space motion for a specified 
cyclic task space motion. 

There are also additional points related specifically to nonholonomic systems: 

• The planner only requires local controllability about a path in every iteration 
but does not require controllability about a configuration. This is important 
since the latter is not satisfied for nonholonomic systems. 

The rest of this paper is organized as follows: Section 2 describes the theory be- 
hind the proposed algorithm. Section 3 describes the global exterior penalty function 
method to handle inequality constraints. Section 4 shows a number of simulation ex- 
amples involving path planning for redundant manipulators and mobile vehicles with 
trailers. 


2 Kinematic Path Planning Subject to Equal- 
ity Constraints 

In this paper, we consider the problem of finding a continuous path that links the 
specified initial and final configurations while satisfying a set of specified equality and 
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inequality constraints (which may be either holonomic or nonholonomic). To state this 
problem precisely, we first represent a kinematic model as a control system: 

q(t) = u(t) ; g(0) = q 0 (1) 

where q G R n is the configuration variable and u G R n is a pseudo-velocity consid- 
ered as the control input. Note that since this is a path planning problem, the path 
variable, f, is arbitrarily normalized to be within the interval [0,1]. There may also 
be additional holonomic and nonholonomic constraints imposed along the path. If the 
constraints along the path are directly incorporated in the kinematics equation, these 
constraints are treated as hard constraints. If the planner iteratively updates the path 
until the constraints are within a certain tolerance, these constraints are considered as 
soft constraints. We first state the general path planning problem with soft constraints: 

Define u = {u(f) : t G [0,1]} and £ = { q(t ) : t G [0,1]}- Given (1), 
find u G L 2 ([0, l];R n ) such that q(t) satisfies (1) and c(q,u) = 0 where 
c : X 2 ([0, 1]; R n ) X L 2 {[ 0, 1]; R n ) -*Y is a given equality constraint function 
for a specified normed linear space , Y. 

The constraint function c may include physical constraints, such as nonintegrable ve- 
locity constraints on wheels (nonholonomic) and contact constraints of manipulators 
(holonomic), or artificial constraint such as the desired end effector path of a redundant 
manipulator. Pathwise holonomic constraints can be represented as 

c(g,u)(t) = k^qit)) , t G [0, 1] (2) 

and pathwise nonholonomic constraints are of the form 

c(£, u)(t) = k 2 (t, q{t), £(t)) , t G [0, 1] (3) 

and can not be integrated to a constraint only involving the configuration variable. 
In many applications, k 2 is linear in q and invariant in t, i.e., k 2 (t,q(t),q(t)) = 
K 2 (q(t))q(t). A system may also be subject to what is known as second order nonholo- 
nomic constraints (for example, for a manipulator with unactuated joints [47]). 

For the path planning problem with hard constraints, the path constraints in (2) 
and (3) are explicitly removed. In the holonomic case, write (2) in the differential form: 

, dk 2 (t,q) u _ ( 4 ) 

dt 8q 

Denote the Jacobian matrix by J(t,q). If J is a fat matrix (as in the case 

of redundant manipulators) and is nonsingular, then the constraint can be explicitly 
incorporated in the kinematics, resulting in 

j = -/+<», <») 

where J + (t,q) denotes the Moore-Penrose pseudo-inverse of J(t, q), J(t t q) is an arbi- 
trary full rank matrix whose range coincides with the null space of J{t,q ), and v is the 
new effective control variable. 
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In the nonholonomic case, if the constraint is linear in u, then the constraint can 
be eliminated, resulting in a new kinematic equation: 

q = K 2 (q)v (6) 

where v E R m is the new effective control variable (m is the dimension of the null 
space of ^ 2(^)5 assuming it is a constant). 

The general planning problem can now be restated as before except the kinematic 
equation is modified as below to incorporate the path constraints: 

«(<) = &(*. «(*)) + /(*> «(*))«(<) ; g(o) = ?o- (7) 


The drift term, fi(t,q), is zero in the case of nonholonomic constraints linear in q. The 
equality constraint function may involve only the end configuration: 


c(q,u) = q( 1) - q d 


( 8 ) 


where q d is the desired final configuration. In the case of redundant manipulators, joint 
cyclicity implies q d = go- 

For the path planning problem as formulated above with either soft or hard con- 
straints, our approach is based on converting the differential description of the problem 
into an algebraic form. First consider the soft constraint formulation. Recall this case 
involves a very simple kinematic equation given by (1). For a given initial configuration 
qo, q can be related to u via a linear causal map, D, and an initial condition vector, 

So’ ... 

q- Du + q^. (9) 

Define the constraint error as 


2 = ciRu + ^u). 


( 10 ) 


The path planning problem is now reformulated as a nonlinear root finding problem 
for c as a function of u Nonlinear root finding has the reputation being numerically 
challenging [48, §9.6]. However, the situation here is different than the general case 
in that the dimension of the search variable, u, is typically much larger than the 
dimension of the constraint equation. Consequently, there are a very large number 
of roots and finding one of these roots is less difficult than the general root finding 
problem. However, equality constraints alone may not produce physically realizable 
solution; we shall see how this formulation can be extended to inequality constraints 
in the next section. 

To find a u that solves y = 0, our basic strategy is to lift a path in Y that connects 
an initial guess y(0) to zero to a path in £2([0> l]> R m )* Then the end point of the path 
in £ 2 ([0, 1]; R m ) is a solution that satisfies the stated equality constraint c(g, u) - 0. 
To achieve this, we set up the path iteration equation: 


dg dg 

dr dr 


( 11 ) 


where 

G = V g c D_ + Vu£. 
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If G is full rank, or, equivalently, the null space of G* (the adjoint map of G) is zero, 
then any one of the following algorithms can be applied to update u : 

£ = + G( ( 12 ) 

ar ar 

where G + is the Moore-Penrose pseudo-inverse of G, and is any desired convergence 
profile of y in r and j^(0) = y(0), for example, we can choose j^(t) = e- QT y(0), 
for exponential convergence. However, this rate may not correspond to the physical 
convergence rate since (12) may take much longer to solve. In practice, Eq. (12) may 
be solved by an ODE solver or discretized with the time step in r found by line search. 

In the hard constraint formulation and with only the end configuration constraint 
(i.e., c(g,u) = ^(l) - qd) , the equality constraint error can be written as 

y = F(u)-q d (13) 

where F maps the control, u, (for a given < 70 ) to the final configuration. The analytic 
form of F is in general difficult to find, and will not be explicitly required. The system 
(7) is globally controllable if and only if the nonlinear map F is onto, and the system 
is locally controllable around u if and only if ^ uF(u) is a linear onto map. Local 
controllability around u is equivalent to the controllability of the linear time varying 
system obtained after linearizing (7) around q and u. The path planning problem can 
be iteratively solved as before using (12) but with 

G = V„F(tt). 

A sufficient condition for convergence is that G is full rank for all t, or, equivalently, 
the time varying linearized system with u(r ) is controllable. For systems without drift, 
such as nonholonomic systems, it has been shown in [43] that this condition is generic. 

Equation (12) can be solved by an ordinary differential equation (ODE) solver with 
an initial guess of u(0) such that the full rank condition is satisfied. In addition, ^(t) 
can be used to check the accuracy of the solution, y(r), at each r. It can be shown 
that if u(0) is sufficiently close to a solution, then the wellposedness of the IVP (and 
its convergence) is assured. As stated before, the wellposedness of the IVP in the 
hard nonholonomic constraint case has been shown to be generic and there has been 
a surge of recent interest of this technical issue [39]. For the general problem, the 
wellposedness condition remains an important research topic. In all of our simulation 
experience involving equality constraints, this has never been a problem. In the next 
section, when constraints are included through the global exterior penalty function, 
the issue of wellposedness becomes more severe. 

An interesting aspect of this approach is that in (12), £ does not affect the guaran- 
teed convergence rate (specified by a), though it does affect the way ji converges. Since 
the dimension of n is much larger than there is much freedom in £ to affect the even- 
tual convergent solution. For example, £ may be chosen so that additional constraints 
in Ji may be satisfied. We have explored choosing £ via quadratic programming with 
some success, but more research needs to be done in exploiting this degree of freedom. 

There are other possible choices of 34 , f° r example, in the soft constraint case. 

§ = G‘^ + G* ( 14 ) 

OX OT 
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du 

dr 


(15) 


G'i 




dw d 

dr 


2 ^f + <?e 


^ = i|NI 2 


The trade-offs between computational load and convergence speeds between these 
schemes are yet to be fully explored. 

The initial guess tt(0) will clearly affect the convergence. We have not extensively 
explored an intelligent procedure (perhaps based on past experience) for this selection. 
In all of our examples, the initial conditions are simply chosen to avoid the rank 
deficiency of G. 

In the algorithm described above, it is critical to be able to calculate G efficiently. 
Since G relates an infinitesimal variation Su to a corresponding infinitesimal variation 
Sc, it can be computed based on the linearized kinematic equation (and the constraint 
equation in the soft constraint case). This procedure is described in detail in [49], and 
has been used in all of our examples. 


3 Kinematic Path Planning Subject to Inequal- 
ity Constraints 

In the previous section, only equality constraints are considered. However, for realistic 
problems, there are also many inequality constraints which need to be enforced. For 
problems involving equality constraints only, the obtained path is frequently undesir- 
able. For example, in the case of a tractor with twin-trailers, the front wheel angles of 
the tractor may go through an unrealistically large range motion, the trailers can be- 
come jack-knifed, and the entire vehicle may go through several complete revolutions; 
in the redundant arm case, the joints can violate joint limits, and there may be self 
collision or collision with other objects in the workspace. Clearly, if this algorithm is 
to be used in practical applications, additional constraints must be incorporated. In 
this section, we present an approach similar to the exterior penalty function method of 
[45] and which has so far been shown to be very effective in addressing the inequality 
constraint issue. 

There are three basic approaches to address the inequality constraints: 

• Convert the inequality constraints into equality constraints by defining a function 
that is zero when the constraint is satisfied and non-zero when the constraint is not 
satisfied. Once this is done, the zero finding approach of the previous section can be 
applied to find a zero solution which corresponds to a feasible path. 

• If the feasible region is convex polyhedral, then linear programming or quadratic 
programming can be used to select the free variable £ in (12) without affecting the 
convergence of the equality constraints. 

• Pre-warp the vector with a multiplicative potential function <j>{q) which is zero on 
the boundary of the inequality constraint and positive inside the feasible region. It has 
recently been pointed out in [44] that as long as the initial path q( 0) lies strictly within 
the feasible region, then the same algorithm (12) can be applied to iteratively move y 
to zero. 

We have extensive experience with the first two approaches. The first approach has 
proven to be very effective,, though there are currently no (genericity) results on the 
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full rank gradient condition as in the equality constraint case. The second approach is 
computationally intensive due to the need to repeatedly solve a constrained optimiza- 
tion problem. It is perhaps best used as a local optimization refinement in conjunction 
with the first approach. The third approach has just recently been suggested and is 
computationally untried; we intend to pursue this direction in the proposed research. 
For the remainder of this section, we will elaborate on the exterior penalty function 
approach. 

Suppose the feasible region is defined by a set of p inequalities: 

5i(2,«) <0 (16) 

where g\ is nonlinear and assumed to be as smooth as needed, and < is interpreted 
as a component-wise relationship. These constraints may be directly placed on the 
configuration variables such as joint limits or on other variables such as the end effector 
of a manipulator, vehicle boundaries, etc. Without loss of generality, we shall consider 
the hard constraint case for the discussion below. Let the relationship between ^ and u 
be denoted by q = F(u). By substituting this relationship, the constraint inequalities 
are transformed to 

g(F(u),u) <0 (17) 

or more simply expressed as 

< 0. (18) 

Suppose now we wish to constrain the states of the system to stay within the feasible 
region defined by the task space. We then define a penalty function corresponding to 
the i th system state as: 


_ V / 7 j« 0 j,U) 9 ji(u) > 0 

' £il 0 sM < o 


(19) 


where A > 1, gj^s correspond to the constraint applied to state i at discrete time j and 
7 i» s are constant weights. This function is nonzero when state i violates the constraints 
at any time along the path, and is identically zero only when all constraints are satisfied. 
For this penalty function, the penalty imposed depends upon the constraint violation 
raised to the A power. Therefore, the penalty function approaches infinity as the system 
makes ever increasing incursion out of the feasible region. Of course many other choices 
for the constraint function are also possible. For example, another penalty function 
which we use frequently is the following: 


z, = rf *.(*)> o 

h\ o #*(■) < o 


( 20 ) 


where r, > 1 and A > 0. This penalty function is bounded by the sum of the 7 j,’s, and 
is therefore in some sense less harsh than the previous penalty function. In practice 
we have seen that the second penalty function gives faster convergence than the first, 
in cases where the vehicle makes large excursions outside of the feasible region. This 
advantage in convergence stems from the fact that is smaller for the second 

constraint function than for the first when large constraint violations occur, causing 
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the IVP to be more wellposed for the second constraint function. For either penalty 
function, the composite constraint vector is defined as z = [ z\ ... z n T . 

We now have a situation similar to the unconstrained case: Find u so that y = 0 
and z- 0. The same approaches described in Section 2 can now be applied. Using the 
hard constraint formulation, the new differential equation in u now becomes: 


dy^r) 

dr 


Gt(u(r)) 


h 

*d 


+ Gi(u(t))((t) 


( 21 ) 


where, 



( 22 ) 


and m and z d describe the desired path in r that links the initial error to zero. The 
gradient of z( u) can be easily obtained via chain rule and the linearized kinematic 
equation [49]. The convergence of this algorithm now requires the full rank condition 
on G\ in each iteration. Note that in (21), there is again a free parameter £(t). The 
extra degrees of freedom offered by f(r) may be used to satisfy additional optimality 
considerations. 

The penalty function formulation we have presented allows for a wide range of 
constraint types. Up to this point we have considered constraints which apply directly 
to the configuration variable of the system, but the formulation allows constraints to 
be applied to non-configuration variables as well. In any practical application it is 
not enough just to constrain the origin of the system inside a feasible region. The 
boundaries of the system must stay inside as well. For instance, for a single planar 
body, we relate any point p on the boundary to the configuration variable q by the 
nonlinear transformation: 


Px 

Py 


?1 

+ 

cos(g 4 ) -sin(g 4 ) 

r x 

q 2 

_ sin(g 4 ) cos (q 4 ) 

. r y . 


(23) 


where r x and r y are the x and y positions of the boundary point in the body frame. 
The same penalty function which applied to q before now applies to p, gj{p(qj («)))• 
A similar transformation can be obtained to relate boundary points of multi-bodied 
systems to the configuration variable. 

The penalty function formulation also allows for a wide range of feasible region 
types. For instance, when the feasible region is polyhedral, g takes on particularly 
simple form: 

g(q) = Ag-b. 

We have looked at this type of constraint extensively. In fact, this constraint type is 
always used when limiting wheel or jackknife angles on wheeled vehicles. This type of 
constraint is simple to use but the feasible region is necessarily convex: 

The penalty function can be used to enforce both convex and non-con vex, non- 
polyhedral constraints. For instance, supposed it is desired to drive the system while 
keeping boundary points outside of an circular region. The constraint g at each time 
j can then be expressed as: 


9j(p ) = ~(Px - X 0 ) 2 - ( Py - yo) 2 + r 2 
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where xo and yo are constants, r is the radius, and p is a boundary point as described in 
(23). We have had much success in using this formulation with cars and tractor-trailer 
vehicles and present an example in the next section. 

In the preceding discussion, system constraints are expressed as analytic functions 
of the configuration variable. 'But again, in practical situations it may not be possible 
to adequately represent the constraints by analytic functions. In particular, for highly 
unstructured environments it may require great effort to find an appropriate analytic 
function, and once found, it may be computationally intensive to apply it. Therefore, 
we propose a method based upon the contour map built up from the task space obstacle 
boundaries. First, suppose that the feasible region is defined by a set of p inequalities 
of the form: 

92(Z(u)) < 0 (24) 

where g 2 is nonlinear, smooth as needed, and < is component-wise. The gradient of 
02 with respect to u is then obtained by applying the chain rule, 

Vu52 = [V lt Z] T V i 0 2 . (25) 

Note that in this equation, is independent of the constraints and that 

depends solely upon the constraints. Therefore, rather than compute g 2 and V q g 2 
explicitly using analytic functions, it is possible to use a lookup table to compute these 
values. For < 72 , the lookup table contains the cost for each point in a grid covering the 
task space. For the table contains the gradient components for each point on the 
grid. As before, this formulation will also work for non-configuration variables. Since 
the exterior penalty function approach is used, the contour map is zero everywhere 
inside the feasible region and non— zero outside. We have just begun using this method 
for path planning and have so far had good success. An example of the contour map 
method applied to a car is given in the next section. 

In this, and the previous sections, we have used the notation u to denote the set 
of control inputs applied to the system at each point in time along the path. For 
the continuous case this set contains an infinite number of elements. Therefore u in 
vector form would be an infinite dimensional vector. However, in order to implement 
the algorithm on a digital computer u needs to have finite length. The first obvious 
step would be to discretize u(t) using the standard basis. We have used this approach 
extensively and have obtained good results from it. Another approach we have tried is 
to use the first N elements of the Fourier basis to approximate u(t). In this formulation, 
for each discrete time j we represent the control input as: 

uj - ( 26 ) 

where $ is a matrix containing the Fourier basis elements and A is the constant vector 
of Fourier coefficients. In this formulation, A uniquely describes the control over all 
time, since it is independent of time. Now for a given initial configuration g can be 
related to A by a causal map, 

* = Zi(A>. (27) 

Furthermore, substituting for £ and A for U. in all previous equations, we see 
that all previous methods still work. The full advantages and disadvantages in using 


the Fourier basis formulation rather than the discrete basis is unknown at present 
However, in the case of driving a car around a circle, we have observed that the discrete 
basis formulation has great difficulty arriving at a solution whereas the Fourier basis 
formulation solves the problem with relative ease. 


4 Examples 


We have applied the algorithm to a large number of computer simulation examples 
and experimentally to a mobile robot. For illustration purposes, we include several 
examples involving three different wheeled vehicles and a point-to-point path planning 
of a 4R planar arm and a 9DOF arm like the one in our lab (see next section on a 
fuller description of this arm). 

In the first example, we consider the parallel parking of a double tractor-trailer. A 
true double tractor-trailer, like those seen driving on the highways, actually consists 
of a tractor with three trailers: two long trailers connected by a comparatively short 
trailer. The short trailer, or dolly, makes any backing-up situation extremely difficult 
in that small backward motions can produce large jackknife angles between the dolly 
and the trailers. In fact, it is known that with a human operator one must, in general, 
disconnect the end trailer and dolly before any backward motions are attempted. 

In the example below, shown in Fig. 1-2 below, the front wheels are constrained 
to ±35° and the jackknife angles (the jackknife angle is defined as the angle between 
the center line of one trailer and the center line of an adjacent trailer) are constrained 
to ±50°. The limits on the jackknife angles are sufficient to ensure that none of 
the trailers collide. In this example the algorithm required about four hundred fifty 
evaluations of the right hand side of the differential equation to achieve a tolerance on 
the potential fields of 0.01. The penalty function method used here is simply the first 
convex, polyhedral method mentioned in the previous section. 
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Figure 1: Parallel Parking Path of a Double Tractor-Trailer with Constraints 

The next example is presented to demonstrate the versatility of the algorithm with 
regard to the type of constraint applied. In this example, it is desired to drive a 
tractor-trailer vehicle around a circular obstacle while remaining within a rectangular 
region limiting motion along the z-axis. To further complicate the example, two other 
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Figure 2: Parallel Parking Path of a Double Tractor-Trailer with Constraints 


circular obstacles are included above and below the first one. In addition, these task 
space constraints are applied to fourteen different non-configuration variable points 
around both the boundary of the tractor and the boundary of the trailer. The steering 
angle is constrained to ±20°, and the jackknife angle is constrained to ±50°. We 
therefore have in this one example, all of the different types of constraints mentioned 
in the previous section: convex polyhedral (steering, jackknife, and x-position), non- 
convex non-polyhedral (circular obstacles), and non-configuration variable (boundary 
point) constraints. The Fourier basis representation of the control input, with thirty 
three basis elements, was also used in this example. Using a steepest descent method 
with a Golden section search, the algorithm took fifty four evaluations of the right hand 
side to converge. The initial path in this case, drives straight through the obstacle. 
The final result is as shown below in Fig. 3. 
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Figure 3: Tractor-Trailer Example with Non-Convex Task Space Constraints 
The next simulation example uses the contour map method to calculate the penalty 
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function. In this example, it is desired to drive a car around a circular obstacle while 
remaining inside of a circular region. Seven points around the boundary of the car are 
used^for non-configuration variable constraints. The front wheels are constrained to 
±15°. The Fourier basis was once again used to represent the control input along the 
path. This example took about one hundred evaluations of the right hand side using 
a discretized version of (21) with Golden section search to find the optimal step size 
The initial path drives straight through the obstacle. Below, the first set of plots, in 
Fig. 4, show the car and path in the context of the task space. The second set of plots, 
in Fig. 5 show the contour map used in this example. 
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Figure 4: Path Planning with Contour Map Method 



Figure 5: Plots of Exterior Penalty Function and Contour Map 
In the final wheeled vehicle example, we present experimental results of applying the 
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path planning algorithm to an actual mobile robot. The robot is actually a one quarter 
scale model car, complete with a passive suspension system, on-board IBM compatible 
486 33-MHz computer, and wheel encoders for use in dead-reckoning feedback. The 
desired path was a simple parallel parking path with constraints only on the steering 
angle of ±15° which is the physical limit of the car. The purpose of this experiment 
was to determine if a real robot can actually follow a path generated by the planning 
algorithm. In this example, the path was generated, and then a velocity profile was 
imposed. A simple, Lyapunov function based, feedback controller was used to try and 
track the path. Although there is some error in tracking the path, it is obvious from 
the plots below, in Fig. 6-7 that the path is such that the real car can follow it. The 
tracking errors are due in part to error in the dead-reckoning feedback and to the 
controller used. 
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Figure 6: Parallel Parking of Catmobile: Experimental vs. Simulation Path 

Various examples of redundant manipulators are presented which incorporate both 
joint and task space inequality constraints. Key features of this approach include vari- 
ous possible goal task specifications (from end-point only to entire path specification), 
joint path cyclicity, and robustness to manipulator singularities. Inequality constraints 
are handled by the global penalty functions and a linear inequality set description as 
shown in Sections 3. Simulations were performed using Matlab with computationally 
intensive routines coded as Matlab-callable C routines (cmex files). Note that the end 
effector constraint is treated as a soft constraint here as compared to the nonholonomic 
constraint considered above which is treated as a hard constraint. 

The first example illustrates the joint sequence for a 3R planar arm required to trace 
out the tip path shown while remaining within a set of task space boundaries (Fig. 8). 
The path shown in Fig. 8 includes an intermediate joint vector in which links 2 and 3 
become aligned - equivalent to a pose switch for spatial arms. Local planning methods 
typically encounter difficulty in handling pose changes since they correspond to arm 
singularities, and the arm Jacobian losing rank. The present algorithm executes the 
pose switch smoothly, while tracking the desired tip path. In the next situation (Fig. 9) 
a 4R arm is required to traverse to specified coordinates Xj . The obstacles are chosen 
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Figure 7: Parallel Parking of Catmobile: Experimental vs. Simulation State Trajectories 

to provide a relatively narrow opening through which the arm must pass. Typically, 
this type of scenario is very challenging for planners based on purely local potential 
field formulations due to a local minimum formed in the space between Obstacles 1 
and 2. By iterating from an intermediate path sequence generated for the goal end- 
point without consideration of the obstacles, it is possible to warp the intermediate 
path to meet all the constraints. It should also be noted that the final path cannot be 
accomplished without switching the pose along the way. 



Figure 8: Cartesian Tip Path Figure 9: Obstacle Avoidance Problem 


The ability to incorporate joint path cyclicity as an equality constraint is illustrated 
by an example where a 3R arm must trace out the boundaries of a Cartesian square 
path, within a restricted workspace (Fig. 10). Path planning for cooperating arms ma- 
nipulating a common payload imposes an additional kinematic closure constraint on 
the arm tips. Fig. 11 shows the path sequence generated for a pair of 3R planar manip- 
ulators required to move the connecting linkage from its initial position of ( — 1, 2, 0°) to 
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a goal position of (2,2,45°). In the planar examples shown, the apparent collisions of 
the links with the obstacle boundaries arise out of the fact that checking is performed 
for the tips of each link only. A more complete collision detection procedure is being 
developed for spatial arm planning. 



Figure 10. 3R Cyclic Path Figure 11: Cooperating Arm Problem 

The remaining examples illustrate applications to a spatial redundant 9 DOF arm. 
This consists of a PUMA 600 manipulator mounted on a platform providing additional 
Unear, rotate and tilt joints. (See [50] for details of the CIRSSE dual-arm robotic 
testbed.) Fig. 12 shows the output sequence for joint path end-point planning to a 
specified task space position/orientation in the presence of an obstacle. Fig. 13 shows 
the path sequence generated for a path foUowing task incorporating a straight Une 
translation coupled with a rotation of 180° about the tip Y axis. Because of the 
manipulator joint Umits, this can only be accompUshed by switching the PUMA’s pose 
from elbow-up to elbow-down at some intermediate point. As in the planar examples, the 
present algorithm accompUshes a smooth transition between these arm configurations. 
The final example (Fig. 14) illustrates a cycUc joint path sequence computed for the 
arm to follow a circular task space path while maintaining a fixed tool orientation 
(perpendicular to the plane of the circle). 

For the cases shown, the planar examples required from seven to ten iterations to 
reduce the task error and meet all the constraints, while the spatial examples ranged 
from ten to twenty-five iterations. Discretization levels ranged from N = 10 to 40 
in size. For the 9 DOF redundant arm planning scenario, this represents a small 
computational load for the Sun SPARC-station used, workstations. For problems of 
larger size, this technique can be applied recursively to the desired discretization level, 
thereby keeping the individual iteration array operations small. The ability to apply 
the algorithm to the global problem (even at the coarsest resolution) is essential for 
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Figure 12: 9 DOF Path Planning Figure 13: 9 DOF Path Following 

developing globally feasible solutions to the constrained path planning problem. 


5 Conclusions 

Kinematic path planning for robots is a key step in their effective utilization on earth 
and in space. The various types of constraints such as holonomic and nonholonomic, 
and equality and inequality constraints pose particular challenges. In this paper, we 
consider a novel and promising method which warps the entire path until all constraints 
are satisfied. The main approach is to convert the differential (local) kinematic rela- 
tionship to an algebraic (global) equation. With emphasis placed on feasibility rather 
than optimality, we obtain an initial value problem rather than two-point boundary 
value problem typically arisen in a global optimization approach. This formulation is 
general enough to include both redundant manipulators and nonholonomic systems, 
and combination of the two; these topics are traditionally treated separately due to 
their unique properties when local algorithms are applied. The inequality constraints 
are handled through a global exterior penalty function method, which allows for non- 
polyhedral constraints as well as constraints on non-configuration variables. For the 
future research, we will address the following fundamental issues related to this promis- 
ing algorithm: 

1. Develop conditions of convergence of the algorithm based on the wellposedness of 
the initial value problem. Also develop strategy to proceed when the algorithm 
fails to converge due to the rank deficiency of G in (12). 

2. Develop an algorithm to adaptively adjust the path discretization step size based 
on the local discretization error. 

3. Incorporate optimality as a secondary criterion. 

4. Improve the sensitivity and robustness of the algorithm with respect to imperfec- 
tion in the kinematic model. 

5. Implement the planner on various experimental platforms. 
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Figure 14: Spatial 9 DOF Cyclic Cartesian Path 
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